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1. Introduction 


Simultaneous Localization and Mapping (SLAM) requires a mobile robot to autonomously 
explore the environment with its on-board sensors, gain knowledge about it, interpret the 
scene, build an appropriate map and localize itself relative to this map. Many approaches 
have been proposed both in the framework of metric and topological navigation. A very 
successful metric method is the stochastic map (26) where early experiments (4) (16) have 
shown the quality of fully metric SLAM. 

Currently, the SLAM has two contrasting problems to be solved, which are often faced with a 
trade-off: 


— The map precision; 
— The computational requirement for real-time/real-world implementation 


Dissanayake et al. (5), proved the convergence of an algorithm based on the Kalman filter 
theoretically. However, the proof is based on the strong hypothesis of a linear observation. 
Julier and Uhlmann (13) and Castellanos et al. (2) proved that the conventional EKF based 
SLAM (from now on EKF — SLAM) yields an inconsistent map (in particular, in (13) was 
shown that this happens even for the special case of a stationary vehicle with no process noise). 
The map inconsistency arises from the linearization introduced by the Extended Kalman Filter 
(EKF) as clearly pointed out by Castellanos et al. (2). Indeed, this approximation only holds 
if the difference between the estimated state and the ground truth is small. Now, in any map 
representation, the corresponding vehicle location will drift (if no loop is closed). This is 
a consequence of the fact that the absolute location is derived from a composition of many 
relative measurements. Therefore, when the drift is large enough, the linearization is not a 
possible approximation. Additionally, even by solving SLAM with an EKF (i.e. by adopting 
a linearization) the computational complexity becomes prohibitive since it grows up squarely 
in the features number. 

In 2006 Eustice et al, (see (6)) introduced an approach called Exactly Sparse Delayed-State 
Filters (from now on ESDF). Basing on the Information Filter (from now on IF or EIF with 
linearization) and only introducing negligible approximations on the the state recovery, 
this technique solves the SLAM problem through a constant-time filtering algorithm. This 
means that the ESDF computational cost does not grow up with the environment size. 
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Therefore, ESDF is considered as the solution to the scalability problem for arbitrately large 
environments. 

Because of this optimal computational behaviour, the ESDF cannot be improved in terms 
of computational cost through a Filter-based solution to SLAM. On the other hand, the EIF 
used by the ESDF method suffers from the same limitation of the EKF-SLAM in terms of map 
accuracy. In particular, as well as the EKF, the EIF is based on the linear approximation of the 
analyzed system. Hence, the estimation process could become inconsistent if the environment 
is large enough. 

Recently, a new strategy has emerged that offers the possibility to solve the SLAM problem 
without any linear approximation. This approach is called Graph-based approach. It consists 
in facing the SLAM as a non linear optimization problem: find the robot trajectory and the map 
with the greatest probability, given the sensor measurements. In the works realized by Olson 
(22) and Grisetti (9) non linear optimization algorithms are proposed in order to solve the 
SLAM problem. These suggested algorithms are able to build very accurate maps, with a low 
computational cost. In the first part of this chapter, we illustrate a new approach to SLAM 
which combines an EIF and a non linear optimizer. In particular, we suggest a hybrid solution 
to SLAM which consists in using a suitable modification of the ESDF filtering algorithm 
when the system non linearities are supposed to be negligible, and switching to a non linear 
optimizer when the system non linearities are stronger (e.g. loop closure). An analogous 
strategy was proposed in (18) in the context of the Relative Map Approach to solve SLAM. 

In the second part of this chapter we will focus our attention on the cooperative case, i.e. 
when the estimation process is performed simultaneously by a team of agents. In particular, 
we consider the case of flying vehicles. In recent years, flying robotics has received significant 
attention from the robotics community. The ability to fly allows easily avoiding obstacles and 
quickly having an excellent birds eye view. These navigation facilities make flying robots 
the ideal platform to solve many tasks like exploration, mapping, reconnaissance for search 
and rescue, environment monitoring, security surveillance, inspection etc. In the framework 
of flying robotics, micro aerial vehicles (MAV) have a further advantage. Due to the small 
size they can also be used in narrow out- and indoor environment and they represent only 
a limited risk for the environment and people living in it. One of the main prerequisite for 
the successful accomplishment of many tasks is a precise vehicle localization. Since micro 
aerial vehicles are equipped with low computational capabilities an efficient solution must be 
able to distribute the computation among all the agents in order to exploit the computational 
resources of the entire team. Distributing the computation has also another key advantage. It 
allows us to make the solution robust with respect to failures. On the other hand, distributing 
the computation must also account the limited communication capabilities. 

The cooperative localization problem has been faced by many authors so far. Fox and 
collaborators (7) introduced a probabilistic approach based on Markov localization. Their 
approach has been validated through real experiments showing a drastic improvement in 
localization speed and accuracy when compared to conventional single robot localization. 
Other approaches take advantage of relative observations for multi-robot localization (8; 10; 
15; 23; 24; 27). In (10) a method based on a combination of maximum likelihood estimation 
and numerical optimization was introduced. This method allows to reduce the error in the 
robot localization by using the information coming from relative observations among the 
robots in the team. In (24), a distributed multi robot localization strategy was introduced. 
This strategy is based on an Extended Kalman Filter to fuse proprioceptive and exteroceptive 
sensor data. In (17), the same approach was adapted in order to deal with any kind of relative 
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observations among the robots. In (24), it was shown that the equations can be written in 
a decentralized form, allowing the decomposition into a number of smaller communicating 
filters. However, the distributed structure of the filter only regards the integration of the 
proprioceptive data (i.e. the so called prediction phase). As soon as an observation between 
two robots occurs, communication between each member of the team and a single processor 
(which could be embedded in a member of the team) is required. The same communication 
skill is required when even an exteroceptive measurements which only regards a single robot 
occurs (e.g. a GPS measurement). Furthermore, the computation required to integrate the 
information coming from this observation is entirely performed by a single processor with a 
computational complexity which scales quadratically with the number of robots. Obviously, 
the centralized structure of the solution in dealing with exteroceptive observations becomes 
a serious inconvenient when the communication and processing capabilities do not allow 
to integrate the information contained in the exteroceptive data in real time. In particular, 
this happens as soon as the number of robots is large, even if each robot performs very few 
exteroceptive observations. In (20) this problem was considered. However, the structure of 
the filter was maintained the same as in (24) (namely centralized in dealing with exteroceptive 
data). Each robot was supposed to be equipped with several sensors and the optimal sensing 
frequencies were analytically derived by maximizing the final localization accuracy. The limit 
of this approach is that as the number of robots increases, the sensing frequencies reduce. In 
other words, by performing the estimation process in a centralized fashion it is necessary to 
reduce the number of observations to be processed as the number of robots increases. Hence, 
distibuting the entire estimation process can provide a great improvement. 

The information filter is very appealing in this framework since the integration of the 
exteroceptive data is very simple and could be easily distributed. On the other hand, the 
equations which characterize the prediction step are much more complex and their distributed 
implementation seems to be forbidden. This is a serious inconvenient since the proprioceptive 
data run at a very high frequency. 

Eustice et al. (6) and Caballero et al. (1) have recently shown that by using a delayed state 
also the prediction step has some nice properties. In particular, in (6) a solution to the SLAM 
problem by using an Extended Information Filter (EIF) to estimate a delayed state has been 
proposed. In (1) the tracking problem has been considered. 

In the second part of this chapter we present the problem of cooperative localization in 3D 
when the MAVs are equipped with inertial sensors and exteroceptive sensors (e.g. range 
sensors and GPS). We adopt a delayed state and we perform its estimation by using an 
Extended Information Filter. We introduce a simple trick which allows us to mathematically 
express the quantities measured by the IMU (Inertial Measurement Unit) as a function of the 
delayed state (i.e. the state to be estimated). In other words, by using this trick, the link 
between sensor-state for the IMU (which are typically proprioceptive sensors) has the same 
mathematical expression of the one which characterizes an exteroceptive observation. This 
allows us to use the equations of the integration of the exteroceptive data also to integrate the 
IMU data. In this way the equations of the EIF prediction step are never used and the overall 
estimation process can be easily distributed. 

When dealing with a 3D environment, another important issue arises. The orientation of a 
MAV which moves in 3D is provided by 3 parameters. On the other hand, the MAV dynamics 
become very easy by adopting quaternions. However, this parameterization is redundant. 
This means that part of the information is frozen in a geometrical constraint. Without using 
this constraint part of the information is not exploited and the overall precision gets worse. 
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To this regard a new filter, the projection filter, has been introduced (21); this filter permits us 
to consider the geometrical constraint (expressing that the quaternion must be unitary) as an 
ideal observation. 


2 A brief overview on extended information filter 


Consider an arbitrary system driven by the discrete equations 


xj = f (xi-1/ui) 
zi = h(x;) 


Let us denote with & and ¢ the information matrix and the information vector respectively; 
we recall that information matrix and information vector are related to covariance matrix P 
and mean value p as follows 

SSP, č=P tų. 


2.1 Estimation with EIF: integration of exteroceptive data 
Let R be the covariance matrix characterizing the measurement error for an exteroceptive 
sensor. The update equations at the time step 7 are (see (28)): 


Xj = X; + Lops, Lobs = H RH (1) 


Či =i + Cons, Cops = Hf Ro" [z; — h(F;) + HR], @) 


= : . . : ; : = eel ee 
where %j,¢; are the predicted information matrix and information vector, #; =X; ¢; is the 
predicted mean value and H; is the Jacobian of the observation function h(-) evaluated at 7/;. 


2.2 Estimation with EIF: integration of proprioceptive data 
Denoting by Q a noise term affecting the system dynamics, the prediction steps are given by 


Lj = etal + Q| as (3) 


È = LEL bic (4) 
where F; is the Jacobian of the dynamics f(-,-) evaluated at the estimated mean value 


(Hi-1,Uj;), where pj_1 = reDé S. 


2.3 EIF and delayed-states 

In a multi robot scenario & and ¢ characterize the probability distribution of several robots; in 
(1) it is shown that delayed-states allow us to distribute the estimation process over the entire 
network. In particular the authors explain how to recover the global belief from the local belief 
of each network node and remark that the same operation with standard (non delayed) states 
is not possible at all. 


Definition 1 A delayed-state is a dynamic vector X whose entries at time step i are the current robot 
coordinates x; together with all the past poses xq,X},.-.,Xj—1- 
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For example, a delayed-state for a 2D robot having coordinates assigned by x; = (1x %y iri) 
is given by the vector 


Xi = (1x0, y,0/90,1x,1/1y,1/ 91, aay Vyisly,is 0i). 


As already mentioned, a distributed algorithm for the implementation of update equations 
(1)-(2) can be designed (see (1)). The structure of such equation is very simple as the 
update consists only in summing the new information from the exteroceptive sensors to the 
predicted values. On the other hand, the prediction equations (3)-(4) are more complicated 
and they cannot be easily distributed. Nevertheless we will show that, once a delayed-state 
is considered, data obtained from proprioceptive sensors can be integrated using only the 
update equations (1)-(2). 


3. SLAM problem for a single 2D robot 


3.1 Advantages and drawbacks in the ESDF-approach 

In 2006 Eustice et al. (6) introduced the innovative technique called Exactly Sparse 
Delayed-state Filters. The ESDF algorithm succeeds in exploiting the benefits of the EIF 
by maintaining a sparse structure of the information matrix (covariance inverse), without 
any approximation. This is obtained through a state-augmentation technique and yields a 
constant-time computational cost per iteration. In the following we will summarize the ESDF 
method pointing out some of its key properties. 

Let us represent the robot motion and perception by the following equations: 


xi = f (Xi—1,Ui + wi) (5) 
z; = h(x; m) + v; (6) 


where x; is the robot pose at the time step i, u; is the control input (proprioceptive 
measurement), z; is the exteroceptive measurement available at the time step i, m is the 
environment map, f is the robot motion function, h is the observation function, and w; and v; 
are the proprioceptive and exteroceptive measurements errors, respectively. 
The ESDF key idea is to extend the estimated state vector each time an observation occurs. 
Specifically, at the time step 7 the current estimated vector is: 

xP =(aT M) O) 
where M is a vector carrying all the maintained old poses and the map m. In the following we 
will often talk about the size of X; as the environment size. 
Basing on the information form and the state augmentation, the ESDF technique solves the 
SLAM problem by performing the following tasks: motion update, state augmentation and 
observation update. If we suppose that the current state mean p; is available at each iteration 
(i.e. the state recovery problem is supposed to be solved), the three mentioned tasks have 
constant-time computational costs (i.e. independent of the environment size). This is possible 
thanks to the estimated state structure, defined in (7). For a detailed proof, the reader is 
referred to (6). 
On the other hand, the ESDF suffers from a strong limitation about the map precision. 
To be more precise, it suffers from the same limitations of every Gaussian-Filter-based 
solution to SLAM. The crucial problem is that a Gaussian-filter generally is a linear estimator. 
Unfortunately, the SLAM is a strong non linear problem, i.e. the robot motion function f in (5) 
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and the observation function h in (6) are strongly non linear. This leads to the use of the linear 
approximation of both the robot kinematics and observations. In this way the estimation 
accuracy is obviously made worse. 


3.1.1 Estimation process with the EIF 
The estimation process is performed using update and prediciton steps given by (1)-(2) and 
(3)-(4). We introduce the two following assumptions. 


Assumption 1 (Sparse Observation) The observation function h only depends on q components of 
X; and q is independent of the size of Xj. 


Assumption 2 (Easy State Recovery) It is possible to recover the estimated state X; (i.e. obtain ui) 
from the information quantities (i, };) with a complexity independent of the size of Xj. 


Under the previous assumptions we obtain the following property characterizing the 
complexity of the observation update. 


Property 1 (Observation Update Complexity) Under the assumptions 1 and 2 the observation 
update defined by the equations (1) and (2) can be computed with a complexity independent of the 
size of Xj, i.e. the observation update has a constant-time cost. 


Proof: if the observation function h depends on q elements of X;, at any time step k, the 
integration of the information from the corresponding measurement requires to update only 


q entries of č; and q entries of ©; (actually even less (4) because of the symmetry of 2). 


Furthermore, the overall complexity is proportional to q°. In the assumption 1 we suppose 
that q is independent of the size of X;. Therefore, if we suppose that the mean value //; is 
available (assumption 2) the cost to implement the equations (1) and (2) is independent of the 
size of X;. In the following we will suppose that the state recovery problem is solved, i.e. we 
suppose that the assumption 2 is always satisfied. In (6) it is shown that it is possible to recover 
the mean value in a constant-time but its value will be approximated (see (6) for more details). 
Moreover, at any time, the robot typically makes a limited number, q, of relative observations 
to individual landmarks, i.e. a limited number of elements of the state X;. This means that the 
assumption 1 is satisfied. From property 1 we obtain that the ESDF observation update task 
has a constant-time computational cost. 


3.2 Using relative coordinates in ESDF 

In this subsection we describe how to use the relative coordinates in the ESDF framework. In 
particular, we define the new coordinates to represent the same quantities estimated by ESDF, 
i.e. the robot poses and the landmark locations. 

Before introducing the new coordinates, we define the structure of the new estimated state as 
it follows: 


D] = (DR'D}") (8) 


where DB contains all the stored robot poses, and DŁ contains all the landmark locations. 
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3.2.1 Robot pose coordinates 
Instead of defining the robot poses in a common global reference frame, each robot pose is 
defined in the frame of the robot at the previous time step. Let us indicate with dk the robot 
pose at the time step k in the reference of the robot at the time step k — 1, i.e. xj = xj_1 ® dÈ, 
where 9 is the composition operator between two robot poses. Therefore, the portion DR of 
the estimated state has the following structure: 
DR = (aP dR yd) 0) 

Now, let us focus on the robot motion function f, defined in (5). It describes the relation 
between the current robot pose x; with the old robot pose xj;_; and the proprioceptive 
measurement u;, which is available at each time step. We can generally express this relation 
in the following way: 

Xi = f(Xi-1, uj + Wj) = Xj-1 Ð (uj + wi) (10) 
We are assuming that the proprioceptive measurements contain the necessary information to 
provide the shift and the rotation of the robot occurred at every step. This is for instance the 
case of the wheel encoders. From the definition of the new coordinates of the robot pose d? 
and the equation (10) it follows that: 


uj = dÈ + wi (11) 


The expression in (11) allows us to consider u; as a measurement of the estimated state. 
The idea is that the proprioceptive measurement can be considered as an observation of the 
estimated state: u; = h(D;) + w; and hence the proprioceptive measurement information can 
be integrated via the observation update defined in (1) and (2), applied to the measurement 
ui. Furthermore, in our special case, the measurement function defined in (11) satisfies 
the hypothesis of sparse observation (assumption 1). This means that we can integrate the 
considered information with a constant-time computational cost. 


3.2.2 Landmark coordinates 
Instead of defining the coordinates of each landmark in a global and unique reference frame, 
the new state defines a given landmark by its coordinates in the frame of the robot pose where 


it was observed for the first time. Let us indicate with di the coordinates of the landmark j in 
the reference attached to the robot pose at the time step k, i.e. we suppose that the landmark 
j is observed at the time step k for the first time. When the robot, at a given time step | > k 
observes again this landmark, the relative measurement can be expressed by the following 
expression: 

Z = h(x; mj) (12) 
where mj; represents the coordinates of the landmark j in the global frame. Since we are 
considering a relative measurement, the inputs of the function h in (12) can be expressed 
in any reference frame (provided that it is the same for both inputs). By choosing the frame 
attached to the robot pose x; we have: 


z =h(d8 1 @--- odd’) (13) 


With the exception of the loop closure, the function h depends on a number of elements of the 
estimated state which is independent of the size of the environment. To this regard, a loop 
closure event is defined as follows. 
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Definition 2 (Loop Closure) The loop closure is the re-observation of a landmark after a while large 
enough to have at least one of the two following conditions: 


— l — iie. the number of elements ai, (q =1,...,1 — i) in (13), is large enough to make the execution 
of the observation update not possible in real time; 


— the linearization of (13) makes the estimation process inconsistent. 


In order to detect the previous two conditions we propose the following criteria. 

Regarding the first condition, we simply set a threshold on the computational time. As soon 
as the time required by the information filter to integrate a landmark re-observation exceeds 
this threshold, we consider the re-observation a loop closure. 

Regarding the second condition, we propose the following criterion. Since we base on local 
relative coordinates we expect the innovation norm || z; — h(7;) || will be bounded by a given 
threshold. Once defined g? as the max eigenvalue of the innovation covariance matrix, a 
possible threshold value can be 2c. Indeed, in a linear estimation process we know that 
the mentioned norm is bounded by 2c with 0.95 likelihood. On the other hand, the non 
linearities could lead the innovation norm to overtake this threshold. When the norm is 
really bounded we are sure about the estimation consistency. On the contrary, when the 
innovation norm overtakes the threshold, we cannot say the same. Thus, this overtake event 
can be considered as a loop closure. Unfortunately, since we base on the information filter, 
the innovation covariance matrix is not available. However, basing on the measurements 
covariance matrices, we can build an approximated innovation covariance matrix whose norm 
is larger than that of the real one. In this way, we have a consistent threshold since it is larger 
than the theoretical one. 

When a loop is closed, new coordinates corresponding to the re-observed landmark are 
introduced in the state. They are the coordinates of the landmark in the frame of the robot 
pose where the landmark is re-observed. Thus, in this approach there are landmarks whose 
configuration is defined in more than one frame. This means that there are geometrical 
dependencies among state elements. These geometrical dependencies contain the information 
gained at the loop closure. We can say that by adding redundant coordinates we just freeze the 
loop closure information in these geometrical dependencies. This allows us to maintain the 
estimation process of the relative coordinates consistent and totally unaffected by the system 
non linearities. The exploitation of the information at the loop closure, namely of the previous 
geometrical dependencies, will be performed separately by a suitable non linear optimizer. 
We point out that this optimization can be computed only once, even if more than one loop 
closure event occurs. 


3.3 Combining ESDF with a non linear optimizer 

The basic idea consists in introducing a cost function. Such a cost function must carry the loop 
closure information, which is kept by the geometrical dependencies among the estimated state 
elements. Hence, it must be based on this geometrical information. 

In order to simplify the notation, let us indicate the estimated state D; with r, the 
corresponding mean value with f and the information vector and matrix with ¢ and È, 
respectively. Furthermore, we indicate with P the estimate error covariance matrix (i.e. 
inverse of Ł). We remark that both ¢ and & are provided by our ESDF modification 
algorithm. Let us focus on the example represented in figure 1. When the robot re-observes 
a landmark a loop is closed. The blue edges and the red dashed one represent all the relative 
quantities carried by the estimated vector r. The quantity represented by the red dashed 
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Fig. 1. The loop closure information. The blue discs represent the landmarks, the triangles 
represent the robot poses, the edges (blue and red dashed) represent the relative coordinates 
stored in the estimated state. 


edge can be expressed as a function of some of the other quantities, i.e. there are geometrical 
dependencies among state elements. In order to exploit this information, we introduce a new 
state containing only independent quantities. Possible choices are: 


— the independent relative coordinates (e.g. the ones represented by the blue edges in figure 
1); 
— the global coordinates of both robot poses and landmarks in a common frame. 


Let us indicate this state with t. As said, the quantities in r can be expressed as a function of 
the components of T. Let us indicate this function with y (7T) (i.e. r = (T)). 

Our goal is to evaluate T starting from & and ¢. Let us indicate the best evaluation of t with 
Thest- Thest Minimizes the following cost function: 


c(t) =(#-(t))’ P7 (F— p(t) (14) 


namely Tyes = argminzc(T). 
By expanding the expression of c(t) and dropping the part independent of t, we obtain: 


c(t) = p(t) = p(t) — 2p(t)"S (15) 


This last expression is very important since it shows that the computation of the cost function 
is based on the information quantities ¢ and X, namely it does not require to invert È}. 

Our method can now be completed by optimizing the cost function in (15) through a suitable 
optimization method. Literature provides lots of methods able to find a local minimum (or 
maximum) for a non linear function. We decided to use the well known quasi-Newton. 

In order to use an optimization method we need to provide the cost function (15) computed 
for a given value of t and the corresponding gradient. To do this, we must exactly define the 
meaning of the tT components and find the relation expressed by the function y (T). 

For our simulation, whose results can be found in section 5, we defined Tt as the global 
coordinates of both the robot poses and the landmarks in a common frame. Therefore, 
the function (Tt) we obtained is made by inverse compositions which return the relative 
coordinates, given the absolute coordinates in t. Moreover, we observed that such a function 


www.intechopen.com 


158 Multi-Robot Systems, Trends and Development 


is linear on the robot and landmarks location components of t, and non linear on the robot 
orientation components. This makes the cost function in (15) quadratic for the first mentioned 
portion of t and non linear for the second portion. Thanks to this particular property, we 
minimized on the first portion through a suitable algebraical method (i.e. by solving a system 
of linear equations). Then, we minimized on the second portion through the non linear 
optimizer. This algebraical manipulation reduced the dimension of the space in which the 
optimization algorithm had to move, making the optimization significantly faster. 


4. Localization problem for MAV systems 


4.1 Asingle MAV system 

We provide here a mathematical description of our system. We introduce a global frame, 
whose z-axis is the vertical one. Let us consider a MAV equipped with IMU proprioceptive 
sensors (an accelerometer and a gyroscope) as well as some suitable exteroceptive sensors 
(GPS, range sensors). In the following we assume that the IMU data are unbiased. From 
a practical point of view, unbiased data can be obtained by continuously calibrating the 
IMU sensors (see for instance (11)). The configuration of the MAV is described by a vector 
(r,v,0) € R? where r = (Tx, fy,"z) € R is the position, v = (Ux, Uy, vz) € R? is the speed and 
0 = (6;,0p,0y) € R? assignes the MAV orientation: 6, is the roll angle, Op is the pitch angle and 
Oy is the yaw angle. We will adopt lower case letters to express a quantity in the global frame, 
while capital letters for the same quantity exptressed in the local frame (i.e. the one attached 
to the MAV).The system description can be simplified adopting a quaternions framework. We 
recall that the quaternions space H is the noncommutative set of elements 


H= {a +qxi+gyj+qzk: qudxdyqzER, P=} =k = ijk -1}. 
For an arbitrary quaternion q = qt + qxi + qyj + qzk, we define the conjugate element q* = 


qt — qxi — qyj — qzk and the norm ||q|| = qq" = V/4"q = Va + 92+ qy + qZ- 
Denoting by A,Q the accelerometer and the gyroscope values respectively and by ag the 


gravity acceleration (i.e. ag = — (0,0,8) with g ~ 9.81m/s?), the continuous-time dynamics 
of the MAV is given by the following system of ordinary differential equations 
¿=v (16) 
ù=q: A- q“ + ag (17) 
A 1 
G=59°0 (18) 


where r,v,Q,A are purely imaginary quaternions, while q is a unitary quaternion. The 
following relations for roll, pitch and yaw angles 0,, Op, Oy hold 

Gtx + QyQz 
1 — 2(q% + qy) 


r 


Op = 4t4y — 9x9z 


Gt4z + Fy 9x 
1 — 2(q5 + 42) 


by = 
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During the exploration, the MAV performs measurements thanks to its exteroceptive sensors 
equipment; such measurements can be individual (i.e. GPS-based measurements) as well as 
relative to other MAVs poses or to the position of fixed landmarks. The general single MAV 
observation equation is given by 

z=h(r,v,q) (19) 
where h(-,-,-) is a known function. 
In the case the exteroceptive sensor is a GPS, the observation equation is very simple as it is 
linear 

ZGPS =f. (20) 


4.1.1 Estimation with the EIF: the integration of the proprioceptive data 
Let us introduce the delayed-state 


Xi = (Vo GorT 9779) 


containing all MAV poses until the i-th time step. The discretization of the dynamics equations 
over a At time-step interval gives 


Na. = fit oj At (21) 
i+At 
Vit. = Oj +i: f Adt - qj + agAt (22) 
1 i+At 
fi+1 = Fit 541i’ I Odt (23) 


From Equation (21) we can get 
vi = (Ti+, — 17;)/At 
and hence the following recursive formula holds 


i+At 
rig. = 2rj —ri-1 + At (a i f Adt - q; + agat) , (24) 
1 


corresponding to a second order continuous-time evolution. The system dynamics can be 
written in terms of delayed-states as 


Xia = F (Xi), 
where F is a suitable function obtained from (23)-(24). Setting 
- i+At 
fe f Adt (25) 
1 
and 
- i+At 
or f Out, (26) 
1 


the proprioceptive measurements can be regarded as delayed-state dependent functions: 
_ gi (—agAP +7; — ria + 11-2) 4 

At 
Ô; = ho (4i-1,91) = 2471 (1i — qi-1)- 


A; = hA (fi-zfi-1fiqi) 


www.intechopen.com 


160 Multi-Robot Systems, Trends and Development 


In other words, A; and Q; are functions of the state X; to be estimated; moreover, since we 
are considering the discrete dynamics given by (23)-(24), there is no need to include the MAV 
speed v into the state vector X;. 

Due to these considerations, we are allowed to integrate proprioceptive data using (1)-(2) 
instead of (3)-(4), with a consequent reduction of computational cost in the estimation 
algorithm. 

For nonlinear measurements equation (2) involves the mean value and hence information 
matrix inversion is required; nevertheless in many situation, due to the sparsity of such 
matrix, a partial state recovery is sufficient in order to guarantee a good estimate (see (6)). 
Whole state recovering can be obtained using for example the Conjugate Gradients algorithm 
(see (25)) or the Givens rotations factorization (see (14)). We point out that at any update step, 
i.e. when a true exteroceptive measurement is performed, the size of the delayed-state vector 
X increases of 3+ 4=7. 


4.1.2 Projection filter: integration of ideal constraints 

As mentioned in the introduction, the quaternion structure is redundant for the problem we 
are considering and this may lead to a loss of information. To avoid this problem we have 
assumed that the quaternion q is unitary. On the other hand, if the discrete dynamics (23) is 
considered, such property is no longer preserved. Anyway, we can take into account the norm 
invariance of q; imposing an ideal constraint with a fake observation given by the function 


ho(q) =1—4¢ + qx + qy + 42% 
in other words, we can regard the norm constraint as the measurement 
zi = ho(qi) = 0. 


Integration of such fake measurement can be performed with the projection filter (see (21)). 


4.2 The cooperative case 
We consider now the problem of cooperaive localization for a multi robot system. 


4.2.1 The system 

We consider now a fleet of N > 1 MAVs, each one having the characteristics described in 
Section 4.1. Let us denote by (r“), q‘*)) the coordinates of the k-th MAV; the discrete dynamics 
is given by 


i+At 
pV] 9, r + At («!" . i AM dt. (q\)* + agat) (27) 
1 


i+1 i 


k k 1 k i+At k 
PaP aP a (28) 

Each MAV, in addition to the measurement model (19), may perform relative observation; the 

general multi robot observation equation can be written as 

k 1 1 k k N N 

( ) _ ple) (r! gs ) sats ) qf rl das )), (29) 

Simple and common examples of relative observations are distance measures. If the k-th MAV 

measures its own distance from the j-th MAV, the observation is given by 


(k) _ 
Z = GF ix 


Z 


(k) = 7)? + (r® r02 ie (r® _ 72, 


iy iy iz i,z 


www.intechopen.com 


Cooperative Localization and SLAM Based on the Extended Information Filter 161 


4.2.2 The distributed EIF 

When the exploration starts, each MAV begins to integrate the information provided by its 
own sensors by equation (1)-(2) as described before. In particular for any measurement, 
the incoming data are stored in the bottom-right block of the information matrix and, as a 
consequence, in the last entries of the information vector: 


Ej o7(i—1)x7 o7(i—3)x7(i-3) g7(i—3)x21 
ina >= + 
07x7(i-1) 07x7 021x7(i-3) Zo 
Ap 07(i=3)x1 
Či-1 > i= E a 
ox Cobs 


Suppose that after 7, updating time-steps for the j;-th MAV and iz steps for the j2-th MAV a 
relative measurement occurs and for sake of semplicity suppose that j4 < j2. Each MAV has 
to increase the size of the information matrix and information vector in order to store the new 
data. The process is carried out following the steps described below: 


1. State augmentation. The states of the two MAVs are increased in order to have the same size 
7(i1 + i2); this can be done adding a suitable number of zeros in the information matrix 
and information vector. 


7i X71 
Dii) re C (jx) ,i1 
Diii T ASSN os 7 Gin), i = , 
072 x71 Q72 x 7i2 072 x1 
0741 x 7i, 074 x Ziz 071 x1 
Xj), in r Tis XTi , E (jə),iz * 
0 X (jp), i2 © (jn),ia 


2. Relative estimation. The information from relative observations are integrated using the 
standard update equations (1)-(2). Correlation between the estimates on the last poses of 
the MAVs may appear, so that the updated matrices may be not block-diagonal. 


Xj) i i Gia) Ar 
Xj) i = 7 Gi) E 
4K * 4k 
* * * 
2 (jp) i. P 1 Sp), > 
7 2 (jn), ip (jn), ia 


3. Data fusion. A communication is established between the MAVs and they exchange their 
stored data. The data fusion scheme is a non negligible theoretical issue: as a matter of fact, 
if the process is carried out taking simply the sum of the contributions from each MAV, 
estimation errors may arise due to adding several times the same information. Following 
(1), we have adopted a fusion algorithm based on a convex combination of the data: 
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X (jy) ir = WEC) i +(1— W)E Cj.) nr C (jr) i E WE) i += w) Eia) 


Di D (L= Eia POEG Saal Gaya + 6G). 

As proved in (12), for any 0 < w < 1, the above convex combinations lead to unbiased and 
consistent estimates, i.e. no overconfident estimate is performed and there is no overlapping 
of information. 


5. Performance Evaluation 


In order to validate our approach we have performed simulations. We have considered a 
conventional scenario defined by a few parameters which regard the robot(s) perception and 
the environment properties. All our simulations are implemented in Matlab and tested on a 
computer with 1 Intel Pentium CPU M 1.70GHz, 512MB of memory. 


5.1 Single robot SLAM 

5.1.1 Simulated scenario 

In our simulations we consider a two-wheels robot moving in a 110mx110m rectangular area in 
which many point landmarks are randomly distributed. Let us indicate the average landmark 
density with pz, the robot average speed with v, and the distance traveled by the robot with 
d. The data associations ere supposed to be given. We consider a robot equipped with wheel 
encoders which provide the proprioceptive measurements. We base on the Chong-Kleeman 
((3)) error model. According to this model, the translation of the right/left wheel as estimated 
by the odometry sensors is generated as a Gaussian random quantity satisfying the following 
relations: 


sp = Sp"R LG phe (30) 
vRIE ~ N(0,K|6p**/*|) 


In other words, both Jp® and dp! are assumed Gaussian random variables, whose mean 
values are given by the actual values (respectively, p°? and 6p"), and whose variance also 
increases linearly with the travelled distance. In our simulation we set K = 0.00001m, which 
corresponds to an indoor environment (19). Finally, the frequency is 100Hz. 

The simulated exteroceptive sensor provides bearings and ranges of the landmarks whose 
distance does not exceed 12m. Furthermore, the sensor angle of view is 360deg. Both the 
bearings and the distances are generated as Gaussian quantities with variances equal to on 
and 0, respectively. The frequency is 0.2Hz. 


5.1.2 Results 

Figures 2(a)-2(b) illustrate the results provided by a given simulation in which the robot closes 
a loop in counter clockwise direction. Let us point out that the loop closure does not consist 
of the trajectory closure but it consists of the re-observation of landmarks located close to the 
starting point. We implemented both our method and the ESDF one. As said in section 3.3, 
the minimization is carried out through a quasi-Newton method. We set og = 1deg, og = 1cm, 
PL = 0.02m7?, v = 1ms—!, d = 180m. The simulation time is T; = 180s. 

Figures 2(a) and 2(b) show the results obtained before the loop closure. In each figure, 
we represent the true robot trajectory and the true landmark locations (ground truth) by a 
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(a) Simulation 1: estimation results of (b) Simulation 1: 
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(c) Simulation 1: estimation results of (d) Simulation 1: ESDF estimation 
our method after the loop closure. results after the loop closure. 


Fig. 2. Pictures for the results of Simulation 1 


solid blue line and cross blue markers, respectively. Moreover, the estimated trajectory and 
landmark locations are represented by a dash-dotted red line and red x-markers, respectively. 
In order to provide quantitative results, we consider the error on the estimated map by 
computing for all the landmarks the distance between the estimated location and the 
corresponding true location. Then, the mean value on all the landmarks is taken. We refer 
to this mean value as the map error (E°! before the loop closure and E?! after the loop closure). 
The behavior of our estimation process and that of the ESDF one are very similar. However, 
the map errors are E}! = 1.30m for our method and EŻ! = 2.02m for the ESDF. Therefore, our 
method shows a better behavior also before the loop closure. 

Figures 2(c) and 2(d) show the results after the loop closure. The correction we obtained 
through the non linear optimizer clearly outperforms the one computed by the ESDF method. 
This is confirmed by the map errors: E?! = 0.15m for our method and E% = 1.01m for the ESDF. 
The total computation time needed for the estimation process is Tc = 16.20s for our method 
(5.45s for the filtering process and 10.75s for the optimization) and Tc = 39.67s for the ESDF. 
The results provided by a second simulation are shown in Figures 3(a)-3(b): the robot closes 
a loop in counter clockwise direction and than goes on re-traversing a region for a long time. 
The parameters of this simulation are: og = ldeg, 7g = 1cm, pr = 0.02m-2, v = 1.2ms™!, 
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(a) Simulation 2: estimation results of (b) Simulation 2: 
our method before the loop closure. 
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(c) Simulation 2: estimation results of (d) Simulation 2: 
our method after the loop closure. 


Fig. 3. Pictures for the results of Simulation 2 


d = 325m. The simulation time is T; = 270s. Figs. 3(a) and 3(b) show the two methods 
before exploiting the loop closure information. Concerning our method, as said in section 
3.3, a loop closure does not necessary activate the optimization . Indeed, in this case the 
estimation process goes on considering the re-observed landmarks as new landmarks. In 
Figure 3-(a) these phantom landmarks are represented by star red markers. As the figures 
clearly show, our estimation process outperforms the ESDF one. This is confirmed by the map 
errors: E}! = 3.17m for our method and E!! = 3.91m for the ESDF. 

Figure 3(c) shows the results obtained through the non linear optimizer which is activated 
only once, after a long time from the first loop closure. Moreover, Figure 3(d) shows the results 
of the correction computed by the ESDF technique after the loop closure. The comparison 
of these two last figures clearly shows the success of our hybrid approach in improving the 
ESDF performances. This is confirmed by the map errors: E?! = 0.38m for our method and 
E!! = 0.58m for the ESDF. 

The total computation time needed for the estimation process is Tc = 46.36s for our method 
(13.67s for the filtering process and 32.70s for the optimization) and Tc = 91.44s for the ESDF. 
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5.2 Cooperative MAV localization 

5.2.1 The simulated environment 

The trajectories of the MAVs are generated randomly and independently one each other. In 
particular, for every MAV, the motion is generated by generating randomly the linear and 
angular acceleration at 100Hz. Specifically, at each time step, the three components of the 
linear and the angular acceleration are generated as Gaussian independent variables with 
mean values fg and fg and with covariance matrices P, and Py. By performing many 
simulations we remarked that the precision of the proposed strategy is almost independent of 
all these parameters. The simulations provided in this section are obtained with the following 
settings: fla = He = [000], 


(5m5?) 0 0 
P, = 0 0 0 
0 0 0 
and 
(10deg s72)? 0 0 
Pa = 0 (10deg s72)? 0 
0 0 (10deg s72)? 


We adopt many different values for the initial MAV positions orientations and speeds. We 
also consider different scenarios corresponding to a different number of MAVs. 

Starting from the accomplished trajectories, the true angular speed and the linear acceleration 
are computed at each time step of 0.01s (respectively, at the time step i, we denote them with 
Of? and A!’"*). Starting from them, the IMU sensors are simulated by generating randomly 
the angular speed and the linear acceleration at each step according to the following: QO; = 


N (Qe, Po,) and A; = N (af nem Aa hPa) where N indicates the Normal distribution 


whose first entry is the mean value and the second one its covariance matrix and Po, and 
Pa, are the covariance matrices characterizing the accuracy of the IMU; finally, Ag is the 
gravity acceleration expressed in the local frame. In all the simulations we set both P4, and 
Pg, diagonal matrices. In the results here provided they are set as follows: 


(0.1ms~?)? 0 0 
P= 0 (0.1ms~*)? 0 
0 0 (0.1ms~*)? 
and 
(10deg s71)? 0 0 
Po; = 0 (10deg s71)? 0 
0 0 (10deg s71)? 


for every step i. 

The MAVs are also equipped with GPS and range sensors. The GPS provides the position of 
the MAV with a Gaussian error whose covariance is a diagonal matrix and whose components 
are equal to 25m. The GPS data are delivered at 5Hz. Finally, the range sensors provide the 
distances among the MAVs at 2Hz and the measurement errors are normally distributed with 
variance (0.01m)?. 
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5.2.2 Results 

We provide some of the results obtained with the previous settings and by simulating N 
MAVs. In particular, we consider the case of N = 3 and N = 5. Furthermore, we consider 
separately the cases when the estimation is performed by only integrating the IMU data, by 
combining the IMU data with the GPS data and by combining all the sensor data. Finally, 
in order to evaluate the benefit of using the projection filter discussed in Section 4.1.2, we 
consider separately the cases when this filter is adopted and when it is not adopted. 

Figs. 5(a)-5(b) show the results obtained with three MAVs. The blue dots represent the 
ground truth. In Fig. 5(a) the magenta dots represent the GPS data and the black circles 
the trajectories estimated by only integrating the IMU data. It is clear that both IMU and 
GPS are very noisy and cannot be used separately to estimate the MAV trajectories. In Fig. 
5(b) the green dots represent the trajectories estimated by fusing the IMU data and the GPS 
data with our proposed approach (EIF and projection filter). Finally, the red dots represent 
the result obtained by also fusing the range measurements. We remarked that the use of the 
range measurements further reduce the error. In particular, for the simulation in fig. 1a-b the 
position error averaged on all the three MAV and on all the time steps is equal to 0.6m without 
the range measurements and 0.45m with them. As expected, this improvement is still larger 
by increasing the number of MAVs. In Figs. 4(c)-4(d) the results obtained by using 5 MAVs is 
shown. The position error obtained by also fusing the range measurements reduces to 0.2m. 
Fig. 4 shows the benefit of using the Projection filter discussed in Section 4.1.2. In particular, in 
Fig. 4(a) the red circles represent the trajectories estimated by fusing all the sensor data and by 
running the Projection Filter at 5Hz while in Fig. 4(b) the red circles represent the trajectories 
estimated without the use of the Projection Filter. As in the previous figures, the ground truth 
is represented with blue dots and the black dots represent the trajectories obtained by a simple 
integration of the IMU data. 
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Fig. 4. Blue points represent true MAVs trajectories, balck circles are the estimated 
trajectories via odometry and red circles are the estimated trajectories with the EIF. Figure 
4(a) represents the simulation of a 3-MAV system; Figure 4(b) represents the same scenario 
without taking into account the information provided by the projection filter. 
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(c) (d) 
Fig. 5. Blue points represent true MAVs trajectories, black circles are the trajectories with only 
odometric estimates, magenta stars are the GPS data, green stars are the trajectory estimates 


without taking into account relative observations and red circles are the estimates with the 


complete distributed EIF. Figures 5(a)-5(b) are the simulation of 3-MAV scenario, while in 
Figures 5(c)-5(d) is plotted the evolution of a 5-MAV system. 
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6. Conclusions 


In this chapter we considered the problem of cooperative localization and SLAM by using an 
Extended Information Filter. 

We started by considering the ESDF technique (6) which makes possible a 
real-time/real-world implementation for any kind of environment. The only drawback 
of this technique is the use of the linear approximation which could become not consistent 
when the environment is large enough. 

Therefore, we proposed a method able to combine a suitable modification of the ESDF with 
a non linear optimizer. This solution allows us to use the modified ESDF when the non 
linearities are negligible and to switch to the optimizer when the non linearities are not 
negligible. 

Furthermore, we considered the cooperative case, i.e. when the estimation process is 
performed simultaneously by a team of agents. In this case, two original contributions 
have been introduced. The former consists of a simple trick which allowed us to avoid 
the equations which characterize the prediction phase of the extended information filter. 
In particular, the information contained in the data provided by the inertial sensors is 
exploited by using the equations which characterize the perception step of the EIF. This 
allowed us to easily distributing the entire estimation process over all the team members. The 
latter contribution is the use of a projection filter which allowed exploiting the information 
contained in the geometrical constraints which arise as soon as the MAV orientations are 
characterized by unitary quaternions. 

The performance of the proposed approaches has been evaluated by using synthetic data. 
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